#ifndef ROSCONNECTOR_H
#define ROSCONNECTOR_H

#include "ros/ros.h"
#include"sensor_msgs/Joy.h"
#include"sensor_msgs/Image.h"
#include "viewer.h"
#include <QThread>
#include <vector>
#include <Eigen/Dense>
#include "dynamixel_sdk_examples/BulkSetItem_four.h"
#include "dynamixel_sdk_examples/BulkSetItem_one.h"
#include "dynamixel_sdk_examples/BulkGetItem_five.h"
#include "dynamixel_sdk_examples/BulkGetItem_Port3.h"
#include "dynamixel_sdk_examples/BulkGetItem_Port4.h"
#include <std_msgs/Float32MultiArray.h>
#include <std_msgs/Int8.h>
#include"robotKinematics.hpp"
#include "epos_msgs/MotorStatesSRV.h"

class RosConnector
{
private:
    std::vector<DIRECT> signal_msg;
    std::string init_node_name;
    ros::Subscriber m_Sub;
    ros::Subscriber simulation_image_Sub;
    ros::Subscriber simulation_joints_Sub;
    ros::Subscriber maxon_Get;
    ros::Publisher m_Pub_D;
    ros::Publisher m_Pub_Lift;
    ros::Publisher m_Pub_M;
    ros::Publisher m_Pub_ScrewMotor;
    ros::Publisher Pub_total_position;
    ros::Publisher Pub_total_velocity;
    // ros::ServiceClient bulk_Client;
    ros::ServiceClient bulk_Client_Port4;
    ros::ServiceClient bulk_Client_Port3;
    ros::ServiceClient maxon_Client;
    ros::NodeHandle m_nh;
    
    DIRECT direct;
    
    std::vector<double> m_param;
    std::vector<double> m_param_velocity;
    double m_thx, m_thy, m_thz, m_x, m_y, m_z;
    dynamixel_sdk_examples::BulkSetItem_four bulk_msg_robot;
    dynamixel_sdk_examples::BulkSetItem_one bulk_msg_lift;
    epos_msgs::MotorStatesSRV maxon_srv;
    std_msgs::Float32MultiArray maxon_msg;
    std_msgs::Float32MultiArray total_msg;
    dynamixel_sdk_examples::BulkGetItem_Port4 bulk_Port4;
    dynamixel_sdk_examples::BulkGetItem_Port3 bulk_Port3;
    
    double m1_rec, m2_rec, m3_rec, m4_rec, m_dj_1_rec, m_dj_2_rec, m_dj_3_rec, m_dj_4_rec;
    std::vector<double> *traj_motors;//轨迹规划电机角度

public:
    Viewer* m_view;
    kinematics kine;
    int m_command_flag;
    int dynamixel_mode_P_V;//1为速度模式 3为位置模式，  DYNAMIXEL 舵机     DYNAMIXEL 舵机
    bool manualPattern;
    int i_operate,i_screw,i_log;//进程标识符

    uint32_t image_height;//仿真图像参数
    uint32_t image_width;
    string image_encoding;
    uint8_t image_is_bigendian;
    uint32_t image_step;
    std::vector<uint8_t, std::allocator<uint8_t>> image_data;
    std::vector<double> joint_simulation_position;
	std::vector<double> joint_simulation_position_rec;

    std::vector<double> motors;

    RosConnector(ros::NodeHandle &nh,Viewer*view);

    void call_back(const sensor_msgs::Joy &msg);
    void simulation_image_call_back(const sensor_msgs::Image &msg);
    void simulation_joints_call_back(const std_msgs::Float32MultiArray &msg);
    void maxon_call_back(const std_msgs::Float32MultiArray &msg);
    void get_cartesian(const sensor_msgs::Joy &msg);
    void pub_all_robot(const vector<double> &motor_values);
    void pub_all_lift(const double motor_values);
    void pub_Screw_motor(const int &motor_values);
    void pub_total_position(const vector<double> &msg, int work_target);
    void pub_total_velocity(const vector<double> &msg);
    void check_direct(const sensor_msgs::Joy &msg);
    void velocity_mode();
    void position_mode();

    vector<double> present_position_get();
    vector<double> present_velocity_get();
    vector<double> present_position;
    vector<double> present_velocity;

    void set_manual_start(const vector<double> &msg);

    void traj_plan();

    DIRECT getmsg();
    bool init();
    bool init_finished;

    ~RosConnector();
};

#endif // ROSCONNECTOR_H
